#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S3,     sonarSensor,    sensorSONAR)
#pragma config(Sensor, S4,     irSensor,       sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     frontLeft,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     rearLeft,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     leftLift,      tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     arm,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_1,     rightLift,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     flagSpinner,   tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C3_1,     frontRight,    tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_2,     rearRight,     tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S1_C3_1,    rightLatch,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    scoopCover,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_1,    spinnerLift,          tServoContinuousRotation)
#pragma config(Servo,  srvo_S2_C1_2,    leftLatch,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    servo9,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo10,              tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo11,              tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo12,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	nxtDisplayCenteredTextLine(3, "Servos");
	wait1Msec(750);
	servo [scoopCover] = 230;
	servo [leftLatch] = 252;
	servo [rightLatch] = 10;
	eraseDisplay();
	wait1Msec(750);
	nxtDisplayCenteredTextLine(3, "Lift 1");
	nxtDisplayCenteredTextLine(5, "Orange stops");
	while (nNxtButtonPressed != 3)
	{
		motor [leftLift] = -50;
	}
	while (nNxtButtonPressed != (-1))
	{
	}
	motor [leftLift] = 0;
	eraseDisplay();
	wait1Msec(750);
	nxtDisplayCenteredTextLine(3, "Lift 2");
	nxtDisplayCenteredTextLine(5, "Orange stops");
	while (nNxtButtonPressed != 3)
	{
		motor [rightLift] = -50;
	}
	while (nNxtButtonPressed != (-1))
	{
	}
	motor [rightLift] = 0;
	eraseDisplay();
	wait1Msec(750);
	nxtDisplayCenteredTextLine(3, "Arm");
	nxtDisplayCenteredTextLine(5, "Orange stops");
	while (nNxtButtonPressed != 3)
	{
		motor [arm] = -15;
	}
	if (nNxtButtonPressed == 3) {
		while (nNxtButtonPressed != (-1))
		{
		}
		motor [arm] = 0;
		eraseDisplay();
	}
	nxtDisplayCenteredTextLine(3, "Stored.");
	PlayTone (1000, 70);
	wait1Msec (1000);
	PlayTone (1000, 70);
	wait1Msec (1000);
}
